#include "PBVS.hpp"

using namespace std;
using namespace cv;

PBVS::PBVS()
{
	f = 0.08;
	hro_w =  8.333e-5;
	hro_h = 1.0077e-4;
	u0 = 320;
	v0 = 240;

	K = Mat::eye(3,3,CV_64F);
	K.at<double>(0,0) = f/hro_w;
	K.at<double>(0,2) = u0;
	K.at<double>(1,1) = f/hro_h;
	K.at<double>(1,2) = v0;

	P0 = Mat::zeros(3,4,CV_64F);
	P0.at<double>(0,0) = 1;
	P0.at<double>(1,1) = 1;
	P0.at<double>(2,2) = 1;

}


PBVS::~PBVS()
{

}


int PBVS::simulator()
{
	int ret = 1;



	return ret;
}

int PBVS::C170Project(const Mat P_inp, Mat& p)
{
	int ret = 1;

	//double f = 0.08;
	//double hro_w =  8.333e-5;
	//double hro_h = 1.0077e-4;
	//double u0 = 320;
	//double v0 = 240;

	/*Mat K = Mat::eye(3,3,CV_64F);
	K.at<double>(0,0) = f/hro_w;
	K.at<double>(0,2) = u0;
	K.at<double>(1,1) = f/hro_h;
	K.at<double>(1,2) = v0;

	Mat P0 = Mat::zeros(3,4,CV_64F);
	P0.at<double>(0,0) = 1;
	P0.at<double>(1,1) = 1;
	P0.at<double>(2,2) = 1;*/

	Mat P = Mat::zeros(4,1,CV_64F);
	P.at<double>(0) = P_inp.at<double>(0);
	P.at<double>(1) = P_inp.at<double>(1);
	P.at<double>(2) = P_inp.at<double>(2);
	P.at<double>(3) = 1;

	p = K*P0*P;

	p = p/p.at<double>(2);

	return ret;
}


int PBVS::udpateParam(double f_inp, double hro_w_inp, double hro_h_inp, double u0_inp, double v0_inp)
{
	int ret = 1;

	f = f_inp;
	hro_h = hro_h_inp;
	hro_w = hro_w_inp;
	u0 = u0_inp;
	v0 = v0_inp;

	return ret;
}